#ifndef _MAP_BUILDER_HPP_
#define _MAP_BUILDER_HPP_


#include "3d_party_libraries.h"
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <vector>
std::string submap_pcd_name("/home/xjh/catkin_buildMap/src/build3dmap/processedData/submap");
std::string submap_pose_name("/home/xjh/catkin_buildMap/src/build3dmap/processedData/submapPose");
std::string scan_pcd_name("/home/xjh/catkin_buildMap/src/build3dmap/processedData/scan");
std::string scan_pose_name("/home/xjh/catkin_buildMap/src/build3dmap/processedData/scanPose");
unsigned int scan_pcd_id = 0;
using namespace std;

class MapEdge
{
public:
    MapEdge(int _idFirst, int _idSecond, Eigen::Matrix4f _transformT)
    {
        idFirst = _idFirst;
        idSecond = _idSecond;
        transformT << _transformT(0,0) , _transformT(0,1) , _transformT(0,2), _transformT(0,3)
                   , _transformT(1,0) , _transformT(1,1) , _transformT(1,2), _transformT(1,3)
                   , _transformT(2,0) , _transformT(2,1) , _transformT(2,2), _transformT(2,3)
                   , _transformT(3,0) , _transformT(3,1) , _transformT(3,2), _transformT(3,3);
    }

    int idFirst, idSecond;
    Eigen::Matrix4d transformT;//d
};

//SubMap 就是MapVertex
class SubMap
{
public:
  typedef shared_ptr<SubMap> Ptr;
    SubMap(int _id, VPointCloud _initKeyScan,
           Eigen::Matrix4f _subMapGlobalPose,
           double _keyScanPoseInBuildingrRange,
           int _keyScanNum,
           float _targetResolution=0.5,
           float _sourceResolution=1.0
           )
    {
        id = _id;
        keyScanPoseInBuildingrRange = _keyScanPoseInBuildingrRange;
        keyScanNum = _keyScanNum;
        sourceResolution = _sourceResolution;

        keyScan.push_back(_initKeyScan);
        keyScanAfterTransform.push_back(_initKeyScan);
        keyScanLocalPose.push_back(Eigen::Isometry3f::Identity().matrix());

        subMapGlobalPose = _subMapGlobalPose;

        newScanDeltaLocalPose = Eigen::Isometry3f::Identity().matrix();
        newScanLocalPose = Eigen::Isometry3f::Identity().matrix();
        newScanGlobalPose = _subMapGlobalPose;

        updateSumOfAllKeyScan();//更新被匹配的关键帧
        addMatchedMap(true);

        ndt.setTransformationEpsilon (0.01);//设置迭代结束的条件
        ndt.setStepSize (0.1);//0.1改0.2没影响
        ndt.setResolution (_targetResolution);//0.2在停车场只有10cm柱子的时候比较好，0.5会出现匹配问题
        ndt.setMaximumIterations (30);//30改成5 没影响,耗时不变，都是提前跳出的
        ndt.setOulierRatio(0.45);
        ndt.setInputTarget (sumOfAllKeyScan.makeShared());
    }

    //输入采集到的_newScan输出当前全局位置
    Eigen::Matrix4f getPose(VPointCloud _newScan)
    {
        newScan = _newScan;//备份
        VPointCloud::Ptr filtered_cloud (new VPointCloud);
        downsampleCloud(_newScan.makeShared(), filtered_cloud, sourceResolution);
        ndt.setInputSource (filtered_cloud);

        // Calculating required rigid transform to align the input cloud to the target cloud.
        VPointCloud::Ptr output_cloud (new VPointCloud);
        ndt.align (*output_cloud, newScanLocalPose * newScanDeltaLocalPose);//必须加上预测，否则迭代会很慢，里程计的帧数跟不上激光发布的速度，直接导致建图效果很差

        newScanDeltaLocalPose = newScanLocalPose.inverse() * ndt.getFinalTransformation();
        newScanLocalPose = ndt.getFinalTransformation();
        newScanGlobalPose = subMapGlobalPose * newScanLocalPose;

        showLocalPose();
        return newScanGlobalPose;
    }

    //当调用过getPose,并确认在submap范围内可以调用这个，判断刚才的数据是否需要被添加关键帧.若要添加，则要更新sum等
    bool reflash()
    {
        if(addMatchedMap(false))
        {
            VPointCloud temp;
            pcl::transformPointCloud (newScan, temp, newScanLocalPose);//在需要被添加时再计算这个变换

            keyScan.push_back(newScan);
            keyScanAfterTransform.push_back(temp);
            keyScanLocalPose.push_back(newScanLocalPose);
            updateSumOfAllKeyScan();
            ndt.setInputTarget (sumOfAllKeyScan.makeShared());//这部分耗费，内部有降采样的部分，ndt.setResolution
            return true;
        }
        return false;
    }

    bool checkIfInSubMapRange()
    {
        if(pow(newScanLocalPose(0,3), 2) +
           pow(newScanLocalPose(1,3), 2) +
           pow(newScanLocalPose(2,3), 2) < pow(keyScanPoseInBuildingrRange,2))
            return true;
        else
            return false;
    }

    void setLocalPose(Eigen::Matrix4f _newScanLocalPose)
    {
        newScanLocalPose = _newScanLocalPose;
        newScanDeltaLocalPose = Eigen::Isometry3f::Identity().matrix();
        newScanGlobalPose = subMapGlobalPose * _newScanLocalPose;
    }

    void showLocalPoseDebug()
    {
        Eigen::Matrix3f r_TransformAllmatrix = newScanLocalPose.block<3,3>(0,0);
        Eigen::Vector3f r_ = r_TransformAllmatrix.eulerAngles(0,1,2);//roll pitch yaw 顺序
        Eigen::Matrix3f r_TransformAllmatrixGlobal = newScanGlobalPose.block<3,3>(0,0);
        Eigen::Vector3f r_Global = r_TransformAllmatrixGlobal.eulerAngles(0,1,2);//roll pitch yaw 顺序
        std::cout << std::fixed << std::setprecision(4);
        std::cout << "Local/Global Pose (x,y,th): " << "(" <<\
                     newScanLocalPose(0, 3) << " " << newScanLocalPose(1, 3) << " "<< r_(2) << ") (" <<\
                     newScanGlobalPose(0, 3) << " " << newScanGlobalPose(1, 3) << " "<< r_Global(2) << ")" << std::endl;
        std::cout.unsetf( std::ios::fixed );
    }

    int id;//感觉这个变量没意义
    double keyScanPoseInBuildingrRange;
    int keyScanNum;
    float sourceResolution;
    std::vector<VPointCloud> keyScan;
    std::vector<VPointCloud> keyScanAfterTransform;
    std::vector<Eigen::Matrix4f> keyScanLocalPose;

    Eigen::Matrix4f newScanDeltaLocalPose;
    Eigen::Matrix4f newScanLocalPose;
    Eigen::Matrix4f newScanGlobalPose;//注意这个submap中的变量和mapbuilder里的变量重名了！

    VPointCloud newScan;
    VPointCloud sumOfAllKeyScan;

    Eigen::Matrix4f subMapGlobalPose;

    pcl::NormalDistributionsTransform<VPoint, VPoint> ndt;


private:
    inline void updateSumOfAllKeyScan()
    {
       sumOfAllKeyScan +=  keyScanAfterTransform.back();
    }

    double limitYaw(double value)
    {
      if(value>3.1415926)
        value -= 3.1415926;
      if(value < -3.1415926)
        value += 3.1415926;
      return value;
    }
    //需要添加角度方向的策略
    //这里也可以添加概率地图，以及射线跟踪。
    bool addMatchedMap(bool bForceUpdate=false)//强制更新标志,当强制更新时，会重置内部计数器。
    {
      //use length
      static Eigen::Matrix4f transformBefore = Eigen::Isometry3f::Identity().matrix();
      static double yaw = 0;
      static int couter = 0;

      Eigen::Quaternionf ndt_Q(Eigen::Matrix3f(newScanLocalPose.block(0,0,3,3)));
      double ndt_roll, ndt_pitch, ndt_yaw;
      tf::Quaternion ndt_orientation(ndt_Q.x(), ndt_Q.y(), ndt_Q.z(), ndt_Q.w());
      tf::Matrix3x3(ndt_orientation).getRPY(ndt_roll, ndt_pitch, ndt_yaw);//注意 这个rpy时绕固定轴！

      if(bForceUpdate)
      {
          couter = 0;
          transformBefore = newScanLocalPose;
          yaw = ndt_yaw;
          return true;
      }
      if(couter >= keyScanNum*2)
          return false;
    //因为这个是360度激光，所以不用yaw角的策略
      if(pow(newScanLocalPose(0,3) - transformBefore(0,3), 2) +
         pow(newScanLocalPose(1,3) - transformBefore(1,3), 2) +
         pow(newScanLocalPose(2,3) - transformBefore(2,3), 2) > pow(keyScanPoseInBuildingrRange/keyScanNum, 2)
         || fabs(limitYaw(yaw-ndt_yaw))>3.1415926/6
         )
      {
        transformBefore = newScanLocalPose;
        yaw = ndt_yaw;
        couter++;
        return true;
      }
      return false;
    }

    void showLocalPose()
    {
        static int counter = 0;
        counter++;
        if(counter<20)
            return;
        counter = 0;
        Eigen::Matrix3f r_TransformAllmatrix = newScanLocalPose.block<3,3>(0,0);
        Eigen::Vector3f r_ = r_TransformAllmatrix.eulerAngles(0,1,2);//roll pitch yaw 顺序

        Eigen::Matrix3f r_TransformAllmatrixGlobal = newScanGlobalPose.block<3,3>(0,0);
        Eigen::Vector3f r_Global = r_TransformAllmatrixGlobal.eulerAngles(0,1,2);//roll pitch yaw 顺序

        std::cout << std::fixed << std::setprecision(4);
        std::cout << "Local/Global Pose (x,y,th): " << "(" <<\
                     newScanLocalPose(0, 3) << " " << newScanLocalPose(1, 3) << " "<< r_(2) << ") (" <<\
                     newScanGlobalPose(0, 3) << " " << newScanGlobalPose(1, 3) << " "<< r_Global(2) << ")" << std::endl;
        std::cout.unsetf( std::ios::fixed );
    }
};


#endif
